import copy
import numpy as np
import math
from Source.CWJ.gdpz import pipe_to_pipes

def rotation_3D(pipe_8point, angle): #输入八个点，输出旋转后的八个点
    vector = [0, 0, 1]
    value_11 = vector[0] * vector[0] * (1 - math.cos(angle)) + math.cos(angle)
    value_12 = vector[0] * vector[1] * (1 - math.cos(angle)) +  vector[2] * math.sin(angle)
    value_13 = vector[0] * vector[2] * (1 - math.cos(angle)) -  vector[1] * math.sin(angle)
    value_21 = vector[0] * vector[1] * (1 - math.cos(angle)) -  vector[2] * math.sin(angle)
    value_22 = vector[1] * vector[1] * (1 - math.cos(angle)) + math.cos(angle)
    value_23 = vector[1] * vector[2] * (1 - math.cos(angle)) +  vector[0] * math.sin(angle)
    value_31 = vector[0] * vector[2] * (1 - math.cos(angle)) +  vector[1] * math.sin(angle)
    value_32 = vector[1] * vector[2] * (1 - math.cos(angle)) -  vector[0] * math.sin(angle) 
    value_33 = vector[2] * vector[2] * (1 - math.cos(angle)) + math.cos(angle)
    B = np.array([[value_11, value_12, value_13],[value_21, value_22, value_23], [value_31, value_32, value_33]]) #旋转矩阵
    pipe_8point = np.dot(pipe_8point, B) #旋转后的八个点
    return pipe_8point

def rotation_2D(vector, angle): #将向量在xy平面上旋转指定角度
    angle = angle * math.pi / 180 #将角度转化为弧度
    value_11 = math.cos(angle)
    value_12 = math.sin(angle)
    value_21 = - math.sin(angle)
    value_22 = math.cos(angle)
    rotate_Mat = np.array([[value_11, value_12], [value_21, value_22]])
    xy_vector = vector[0:2]
    rotate_vector = np.dot(xy_vector, rotate_Mat)
    rotate_vector = rotate_vector.tolist()
    rotate_vector.append(vector[2])
    return rotate_vector

def distance_point_line(point, line_points): #求二维平面上点到直线的距离，输入一个点和直线的两个点
    A = line_points[0][1] - line_points[1][1]
    B = line_points[1][0] - line_points[0][0]
    C = line_points[0][0] * line_points[1][1] - line_points[0][1] * line_points[1][0]
    distance = abs(A * point[0] + B * point[1] + C) / math.sqrt(A * A + B * B)
    return distance

def get_cos_value(pi, block):
    pipe_vector = [pi[1][i] - pi[0][i] for i in range(3)]
    block_vector = [block[1][i] - block[0][i] for i in range(3)]
    pipe_length = math.sqrt(sum(e**2 for e in pipe_vector))
    block_length = math.sqrt(sum(e**2 for e in block_vector))
    cos_value = np.dot(pipe_vector, block_vector) / pipe_length / block_length
    return cos_value

def get_Unit_vector(vector): #获取单位向量
    length = math.sqrt(sum(e**2 for e in vector))
    unit_vector = [vector[i]/length for i in range(3)]
    return unit_vector

def get_Unit_vector_2D(vector):
    length = math.sqrt(sum(e**2 for e in vector))
    unit_vector = [vector[i]/length for i in range(2)]
    return unit_vector

def getPoints(block_8point): ##获取对角线上的五个点
    diagonal_vector1 = [block_8point[6][i] - block_8point[0][i] for i in range(3)]
    diagonal_vector2 = [block_8point[2][i] - block_8point[4][i] for i in range(3)]
    diagonal_length = math.sqrt(sum(e**2 for e in diagonal_vector1))
    unit_diagonal_vector1 = get_Unit_vector(diagonal_vector1)
    unit_diagonal_vector2 = get_Unit_vector(diagonal_vector2)
    mid_point = [(block_8point[0][i] + block_8point[6][i])/2 for i in range(3)]
    point1 = [block_8point[0][i] + unit_diagonal_vector1[i] * 0.25 * diagonal_length for i in range(3)]
    point2 = [block_8point[0][i] + unit_diagonal_vector1[i] * 0.75 * diagonal_length for i in range(3)]
    point3 = [block_8point[4][i] + unit_diagonal_vector2[i] * 0.25 * diagonal_length for i in range(3)]
    point4 = [block_8point[4][i] + unit_diagonal_vector2[i] * 0.75 * diagonal_length for i in range(3)]
    return [point1, point2, point3, point4, mid_point]

def block_dian2(block):#障碍物标准形式转为八个点
    p1 = block[0]
    p2 = block[1]
    width = block[2]
    height = block[3]
    u=0
    p3=np.zeros(3)
    p4=np.zeros(3)
    p5=np.zeros(3)
    p6=np.zeros(3)
    p_1=np.zeros(3)
    p_2=np.zeros(3)
    p_3=np.zeros(3)
    p_4=np.zeros(3)
    p_5=np.zeros(3)
    p_6=np.zeros(3)
    p_7=np.zeros(3)
    p_8=np.zeros(3)
    if p1[1]-p2[1] == 0:
        u=1
    if u==0:
        k=-(p1[0]-p2[0])/(p1[1]-p2[1])
        x=width/(2*(1+k**2)**0.5)
        p3[0]=p1[0]+x
        p3[1]=p1[1]+k*x
        p3[2]=p1[2]
        p4[0]=p1[0]-x
        p4[1]=p1[1]-k*x
        p4[2]=p1[2]
        p5[0]=p2[0]+x
        p5[1]=p2[1]+k*x
        p5[2]=p2[2]
        p6[0]=p2[0]-x
        p6[1]=p2[1]-k*x
        p6[2]=p2[2]
    else :
        p3[0]=p1[0]
        p3[1]=p1[1]+width/2
        p3[2]=p1[2]
        p4[0]=p1[0]
        p4[1]=p1[1]-width/2
        p4[2]=p1[2]
        p5[0]=p2[0]
        p5[1]=p2[1]+width/2
        p5[2]=p2[2]
        p6[0]=p2[0]
        p6[1]=p2[1]-width/2
        p6[2]=p2[2]
    a=(p2[1]-p1[1])*(p3[2]-p1[2])-(p3[1]-p1[1])*(p2[2]-p1[2])
    b=(p2[2]-p1[2])*(p3[0]-p1[0])-(p3[2]-p1[2])*(p2[0]-p1[0])
    c=(p2[0]-p1[0])*(p3[1]-p1[1])-(p3[0]-p1[0])*(p2[1]-p1[1])
    t=height/(2*(a**2+b**2+c**2)**0.5)
    p_3[0]=p3[0]+a*t
    p_3[1]=p3[1]+b*t
    p_3[2]=p3[2]+c*t
    p_2[0]=p3[0]-a*t
    p_2[1]=p3[1]-b*t
    p_2[2]=p3[2]-c*t
    p_4[0]=p4[0]+a*t
    p_4[1]=p4[1]+b*t
    p_4[2]=p4[2]+c*t
    p_1[0]=p4[0]-a*t
    p_1[1]=p4[1]-b*t
    p_1[2]=p4[2]-c*t
    p_7[0]=p5[0]+a*t
    p_7[1]=p5[1]+b*t
    p_7[2]=p5[2]+c*t
    p_6[0]=p5[0]-a*t
    p_6[1]=p5[1]-b*t
    p_6[2]=p5[2]-c*t
    p_8[0]=p6[0]+a*t
    p_8[1]=p6[1]+b*t
    p_8[2]=p6[2]+c*t
    p_5[0]=p6[0]-a*t
    p_5[1]=p6[1]-b*t
    p_5[2]=p6[2]-c*t
    list = [p_3,p_7,p_6,p_2,p_4,p_8,p_5,p_1]
    new = []
    for i in list:
        a = np.ndarray.tolist(i)
        new.append(a)
    return new

def pipe_dian2(pipe): #管道标准形式转为八个点
    p1 = pipe[0]
    p2 = pipe[1]
    width = pipe[2]
    height = pipe[3]
    angle = pipe[4]
    u=0
    p3=np.zeros(3)
    p4=np.zeros(3)
    p5=np.zeros(3)
    p6=np.zeros(3)
    p_1=np.zeros(3)
    p_2=np.zeros(3)
    p_3=np.zeros(3)
    p_4=np.zeros(3)
    p_5=np.zeros(3)
    p_6=np.zeros(3)
    p_7=np.zeros(3)
    p_8=np.zeros(3)
    if p1[1]-p2[1] == 0:
        u=1
    if u==0:
        k=-(p1[0]-p2[0])/(p1[1]-p2[1])
        x=width/(2*(1+k**2)**0.5)
        p3[0]=p1[0]+x
        p3[1]=p1[1]+k*x
        p3[2]=p1[2]
        p4[0]=p1[0]-x
        p4[1]=p1[1]-k*x
        p4[2]=p1[2]
        p5[0]=p2[0]+x
        p5[1]=p2[1]+k*x
        p5[2]=p2[2]
        p6[0]=p2[0]-x
        p6[1]=p2[1]-k*x
        p6[2]=p2[2]
    else :
        p3[0]=p1[0]
        p3[1]=p1[1]+width/2
        p3[2]=p1[2]
        p4[0]=p1[0]
        p4[1]=p1[1]-width/2
        p4[2]=p1[2]
        p5[0]=p2[0]
        p5[1]=p2[1]+width/2
        p5[2]=p2[2]
        p6[0]=p2[0]
        p6[1]=p2[1]-width/2
        p6[2]=p2[2]
    a=(p2[1]-p1[1])*(p3[2]-p1[2])-(p3[1]-p1[1])*(p2[2]-p1[2])
    b=(p2[2]-p1[2])*(p3[0]-p1[0])-(p3[2]-p1[2])*(p2[0]-p1[0])
    c=(p2[0]-p1[0])*(p3[1]-p1[1])-(p3[0]-p1[0])*(p2[1]-p1[1])
    t=height/(2*(a**2+b**2+c**2)**0.5)
    p_3[0]=p3[0]+a*t
    p_3[1]=p3[1]+b*t
    p_3[2]=p3[2]+c*t
    p_2[0]=p3[0]-a*t
    p_2[1]=p3[1]-b*t
    p_2[2]=p3[2]-c*t
    p_4[0]=p4[0]+a*t
    p_4[1]=p4[1]+b*t
    p_4[2]=p4[2]+c*t
    p_1[0]=p4[0]-a*t
    p_1[1]=p4[1]-b*t
    p_1[2]=p4[2]-c*t
    p_7[0]=p5[0]+a*t
    p_7[1]=p5[1]+b*t
    p_7[2]=p5[2]+c*t
    p_6[0]=p5[0]-a*t
    p_6[1]=p5[1]-b*t
    p_6[2]=p5[2]-c*t
    p_8[0]=p6[0]+a*t
    p_8[1]=p6[1]+b*t
    p_8[2]=p6[2]+c*t
    p_5[0]=p6[0]-a*t
    p_5[1]=p6[1]-b*t
    p_5[2]=p6[2]-c*t
    list = [p_3,p_7,p_6,p_2,p_4,p_8,p_5,p_1]
    new = []
    for i in list:
        a = np.ndarray.tolist(i)
        new.append(a)

    if angle != None:
        new = rotation_3D(new, angle)
    return new

def blocks_select(pipes, block_arr, temp):
    blocks = copy.deepcopy(block_arr)
    pipes_8point = []
    x_array = []
    y_array = []
    z_array = []
    blocks_selection = []
    for i in range(len(pipes)):
        pipe = pipes[i]
        if len(pipe) == 5:
            pipe_8point = pipe_dian2(pipe) #标准形式转为八个点
        else:
            pipe_8point = block_dian2(pipe)
        pipes_8point.append(pipe_8point)
    for pipe_8point in pipes_8point:
        for points in pipe_8point:
            x_array.append(points[0])
            y_array.append(points[1])
            z_array.append(points[2])
    min_coordinate = [min(x_array), min(y_array), min(z_array)]
    max_coordinate = [max(x_array), max(y_array), max(z_array)]
    area = [min_coordinate, max_coordinate]
    for i in range(3):
        area[0][i] = area[0][i] - temp
    for j in range(3):
        area[1][j] = area[1][j] + temp
    min_x = area[0][0]
    max_x = area[1][0]
    min_y = area[0][1]
    max_y = area[1][1]
    min_z = area[0][2]
    max_z = area[1][2]
    height = max_z - min_z
    if (max_x - min_x) < (max_y - min_y):
        weight = max_x - min_x
        start = [min_x + weight/2, min_y, max_z - height/2]
        end = [min_x + weight/2, max_y, max_z - height/2]
    else:
        weight = max_y - min_y
        start = [min_x, min_y + weight/2, max_z - height/2]
        end = [max_x, min_y + weight/2, max_z - height/2]
    for block in blocks:
        block_point = block_dian2(block)
        diagonal_points = getPoints(block_point) #获取对角线上的五个点
        block_point.extend(diagonal_points) #共13个点
        for i in range(13):
            if (area[0][0] <= block_point[i][0] <= area[1][0]) and (area[0][1] <= block_point[i][1] <= area[1][1]) and (area[0][2] <= block_point[i][2] <= area[1][2]):
                blocks_selection.append(block)
                break
    return blocks_selection

def multi_cross(vector1, vector2):
    return [vector1[1] * vector2[2] - vector2[1] * vector1[2], vector2[0] * vector1[2] - vector1[0] * vector2[2],
     vector1[0] * vector2[1] - vector2[0] * vector1[1]]

def getFaceDirection(pipe_8point):
    x_vector = [pipe_8point[4][i] - pipe_8point[0][i] for i in range(3)]
    y_vector = [pipe_8point[1][i] - pipe_8point[0][i] for i in range(3)]
    z_vector = [pipe_8point[3][i] - pipe_8point[0][i] for i in range(3)]
    unit_x_vector = get_Unit_vector(x_vector)
    unit_y_vector = get_Unit_vector(y_vector)
    unit_z_vector = get_Unit_vector(z_vector)
    return [unit_x_vector, unit_y_vector, unit_z_vector]

def getInterval(points, vector):
    min_value = max_value = np.dot(points[0], vector)
    for i in range(1, 8):
        value = np.dot(points[i], vector)
        min_value = min(value, min_value)
        max_value = max(value, max_value)
    return min_value, max_value

def collision_detection(pipe, block): #长方体与长方体的碰撞检测
    pipe_8point = pipe_dian2(pipe) #获取管道的八个点
    if math.isnan(pipe_8point[0][0]):
        return False
    block_8point = block_dian2(block) #获取障碍物的八个点
    pipe_vector = getFaceDirection(pipe_8point) #取长方体的三个方向矢量
    block_vector = getFaceDirection(block_8point) #取长方体的三个方向矢量
    #当前包围盒的三个方向 相当于取包围盒的三个坐标轴为分离轴并计算投影作比较
    for i in range(3):
        min1, max1 = getInterval(pipe_8point, pipe_vector[i]) #计算当前包围盒在某轴上的最大最小投影值
        min2, max2 = getInterval(block_8point, pipe_vector[i]) #计算另一个包围盒在某轴上的最大最小投影值
        if((max1 < min2) or (max2 < min1)): #判断分离轴上投影是否重合
            return False #若分离开，返回“未碰撞”
    #另一个包围盒的三个方向
    for i in range(3):
        min1, max1 = getInterval(pipe_8point, block_vector[i])
        min2, max2 = getInterval(block_8point, block_vector[i])
        if((max1 < min2) or (max2 < min1)): 
            return False
    for i in range(3):
        for j in range(3):
            cross_vector = multi_cross(pipe_vector[i], block_vector[j])
            min1, max1 = getInterval(pipe_8point, cross_vector)
            min2, max2 = getInterval(block_8point, cross_vector)
            if((max1 < min2) or (max2 < min1)): 
                return False
    return True

def is_point_in_surface(point, surface_4point): #输入点与面，判断该点是否在面内
    surface_vector1 = [surface_4point[1][i] - surface_4point[0][i] for i in range(3)]
    surface_vector2 = [surface_4point[2][i] - surface_4point[1][i] for i in range(3)]
    surface_vector = [surface_vector1, surface_vector2]
    for i in range(2):
        min_value = max_value = np.dot(surface_4point[0], surface_vector[i])
        for j in range(1, 4):
            value = np.dot(surface_4point[j], surface_vector[i])
            min_value = min(value, min_value)
            max_value = max(value, max_value)
        point_value = np.dot(point, surface_vector[i])
        if point_value > max_value or point_value < min_value:
            return False
    return True

def point_8_to_surface_6(block_8point): #站在起点向终点看，分别为前后上下左右
    front_surface = [block_8point[0], block_8point[4], block_8point[7], block_8point[3]] #左上角开始，顺时针
    back_surface = [block_8point[5], block_8point[1], block_8point[2], block_8point[6]]
    top_surface = [block_8point[5], block_8point[4], block_8point[0], block_8point[1]]
    bottom_surface = [block_8point[2], block_8point[3], block_8point[7], block_8point[6]]
    left_surface = [block_8point[1], block_8point[0], block_8point[3], block_8point[2]]
    right_surface = [block_8point[4], block_8point[5], block_8point[6], block_8point[7]]
    return [front_surface, back_surface, top_surface, bottom_surface, left_surface, right_surface]

def get_Intersection_points(pipe, block):
    intersection_points = [] #管道与障碍物的所有碰撞坐标
    pipe_five_edges = pipe_to_pipes(pipe) #获取管道五条棱的方向向量
    block_8point = block_dian2(block) #获取障碍物的八个点坐标
    surface_points = point_8_to_surface_6(block_8point) #获取障碍物的六个面的四个点坐标
    for i in range(len(surface_points)):
        for j in range(len(pipe_five_edges)):
            surface_4point = surface_points[i]
            surface_vector1 = [surface_4point[1][i] - surface_4point[0][i] for i in range(3)]
            surface_vector2 = [surface_4point[2][i] - surface_4point[1][i] for i in range(3)]
            normal_vector = multi_cross(surface_vector1, surface_vector2) #获取面的法向量
            edge_vector = [pipe_five_edges[j][1][k] - pipe_five_edges[j][0][k] for k in range(3)]
            if np.dot(edge_vector, normal_vector) != 0:  #若五条中存在与平面不平行的棱，则存在碰撞坐标
                surface_point = surface_4point[0] #平面上任意一点
                pipe_start = pipe_five_edges[j][0]
                surfacepoint_sub_pipestart = [surface_point[i] - pipe_start[i] for i in range(3)]
                hit = np.dot(normal_vector, surfacepoint_sub_pipestart) / np.dot(normal_vector, edge_vector)
                edgevector_multi_hit = [hit * edge_vector[i] for i in range(3)]
                intersection_point = [pipe_start[i] + edgevector_multi_hit[i] for i in range(3)] #某条棱与某一平面碰撞坐标
                start_proj = np.dot(pipe_five_edges[j][0], edge_vector)
                end_proj = np.dot(pipe_five_edges[j][1], edge_vector)
                point_proj = np.dot(intersection_point, edge_vector)
                flag_in_surface = is_point_in_surface(intersection_point, surface_4point) #判断点是否在面内
                if (start_proj <= point_proj <= end_proj) and flag_in_surface: #判断碰撞点是否在线段内
                    intersection_points.append(intersection_point)
    return intersection_points
                
def nearest_intersection_point(pipe, intersection_points):
    pipe_vector = list(map(lambda x, y: x - y, pipe[1], pipe[0]))
    unit_pipe_vector = get_Unit_vector(pipe_vector)
    min_proj = np.dot(pipe_vector, intersection_points[0])
    min_point = intersection_points[0]
    for i in range(1, len(intersection_points)):
        intersection_proj = np.dot(pipe_vector, intersection_points[i])
        if intersection_proj < min_proj:
            min_proj = intersection_proj
            min_point = intersection_points[i]
    vector = list(map(lambda x, y: x - y, min_point, pipe[0]))
    distance = np.dot(vector, unit_pipe_vector)
    nearest_point = [0, 0, 0]
    for i in range(3):
        nearest_point[i] = pipe[0][i] + distance * unit_pipe_vector[i]
    return nearest_point

def step_divide(xyz_redis, pi_vector_reverse):
    step = [0, 0, 0]
    for i in range(3):
        step[i] = xyz_redis * pi_vector_reverse[i]
    return step
        
def get_Backward_point(nearest_point, pi):
    xy_redis = pi[3] * 2 #在xy面上的回退距离为管道的高度的两倍
    reverse_pi_vector = list(map(lambda x, y: x - y, pi[0], pi[1])) #管道反向向量
    xy_square = math.sqrt(reverse_pi_vector[0] ** 2 + reverse_pi_vector[1] ** 2)
    xyz_square = math.sqrt(reverse_pi_vector[0] ** 2 + reverse_pi_vector[1] ** 2 + reverse_pi_vector[2] ** 2)
    reverse_unit_pi_vector = [reverse_pi_vector[i]/xyz_square for i in range(3)] #管道反向单位向量
    xyz_redis = xyz_square / xy_square * xy_redis #xyz面上的回退距离
    step = step_divide(xyz_redis, reverse_unit_pi_vector) #求坐标轴方向上的回退距离
    backward_point = [nearest_point[i] + step[i] for i in range(3)]
    return backward_point

def get_Backward_point_vertical(nearest_point, pi):
    redis = pi[3] * 2
    reverse_pi_vector = list(map(lambda x, y: x - y, pi[0], pi[1]))
    unit_reverse_pi_vector = get_Unit_vector(reverse_pi_vector)
    step = [unit_reverse_pi_vector[i] * redis for i in range(3)]
    backward_point = [nearest_point[i] + step[i] for i in range(3)]
    return backward_point